// -*- c++ -*-
// Copyright 2008 Isis Innovation Limited
// 
// This file declares the MapPoint class
// 
// The map is made up of a bunch of mappoints.
// Each one is just a 3D point in the world;
// it also includes information on where and in which key-frame the point was
// originally made from, so that pixels from that keyframe can be used
// to search for that point.
// Also stores stuff like inlier/outlier counts, and privat information for 
// both Tracker and MapMaker.

#ifndef __MAP_POINT_H
#define __MAP_POINT_H
#include <TooN/TooN.h>
using namespace TooN;
#include <cvd/image_ref.h>
#include <cvd/timer.h>
#include <set>
#include <stdint.h>
#include <pmmintrin.h>
#include <boost/shared_ptr.hpp>

class KeyFrame;

class MapPoint
{
public:
  // Constructor inserts sensible defaults and zeros pointers.
  inline MapPoint()
  {
    bFixed = false;
    bBad = false;
    nMEstimatorOutlierCount = 0;
    nMEstimatorInlierCount = 0;
    dCreationTime = CVD::timer.get_time();
  };

  TooN::Vector<3> v3WorldPos;
  //TooN::Vector<3> v3Normal2;
  
  // Is it a dud? In that case it'll be moved to the trash soon.
  bool bBad;
  bool bFixed;
  
  // What pixels should be used to search for this point?
  boost::shared_ptr<KeyFrame> pPatchSourceKF; // The KeyFrame the point was originally made in
  int nSourceLevel;         // Pyramid level in source KeyFrame
  CVD::ImageRef irCenter;   // This is in level-coords in the source pyramid level
  uint8_t *pDescriptor;  

  // What follows next is a bunch of intermediate vectors - they all lead up
  // to being able to calculate v3Pixel{Down,Right}_W, which the PatchFinder
  // needs for patch warping!
  
  Vector<3> v3Center_NC;             // Unit vector in Source-KF coords pointing at the patch center
  Vector<3> v3OneDownFromCenter_NC;  // Unit vector in Source-KF coords pointing towards one pixel down of the patch center
  Vector<3> v3OneRightFromCenter_NC; // Unit vector in Source-KF coords pointing towards one pixel right of the patch center
  Vector<3> v3Normal_NC;             // Unit vector in Source-KF coords indicating patch normal
  
  Vector<3> v3PixelDown_W;           // 3-Vector in World coords corresponding to a one-pixel move down the source image
  Vector<3> v3PixelRight_W;          // 3-Vector in World coords corresponding to a one-pixel move right the source image
  void RefreshPixelVectors();        // Calculates above two vectors
  
  // Info for the Mapmaker (not to be trashed by the tracker:)
  std::set<boost::shared_ptr<KeyFrame> > sMeasurementKFs;   // Which keyframes has this map point got measurements in?
  std::set<boost::shared_ptr<KeyFrame> > sNeverRetryKFs;    // Which keyframes have measurements failed enough so I should never retry?
  inline int GoodMeasCount() {  return sMeasurementKFs.size(); }
  
  // Info provided by the tracker for the mapmaker:
  int nMEstimatorOutlierCount;
  int nMEstimatorInlierCount;
  
  // Random junk (e.g. for visualisation)
  double dCreationTime; //timer.get_time() time of creation
};

inline bool IsMapPointGood(const boost::shared_ptr<MapPoint> &p) { return !(p->bBad); }

const float gfColourMapHSV[64][3] = {{1,0,0},{1,0.0937500000000000,0},{1,0.187500000000000,0},{1,0.281250000000000,0},{1,0.375000000000000,0},{1,0.468750000000000,0},{1,0.562500000000000,0},{1,0.656250000000000,0},{1,0.750000000000000,0},{1,0.843750000000000,0},{1,0.937500000000000,0},{0.968750000000000,1,0},{0.875000000000000,1,0},{0.781250000000000,1,0},{0.687500000000000,1,0},{0.593750000000000,1,0},{0.500000000000000,1,0},{0.406250000000000,1,0},{0.312500000000000,1,0},{0.218750000000000,1,0},{0.125000000000000,1,0},{0.0312500000000000,1,0},{0,1,0.0625000000000000},{0,1,0.156250000000000},{0,1,0.250000000000000},{0,1,0.343750000000000},{0,1,0.437500000000000},{0,1,0.531250000000000},{0,1,0.625000000000000},{0,1,0.718750000000000},{0,1,0.812500000000000},{0,1,0.906250000000000},{0,1,1},{0,0.906250000000000,1},{0,0.812500000000000,1},{0,0.718750000000000,1},{0,0.625000000000000,1},{0,0.531250000000000,1},{0,0.437500000000000,1},{0,0.343750000000000,1},{0,0.250000000000000,1},{0,0.156250000000000,1},{0,0.0625000000000000,1},{0.0312500000000000,0,1},{0.125000000000000,0,1},{0.218750000000000,0,1},{0.312500000000000,0,1},{0.406250000000000,0,1},{0.500000000000000,0,1},{0.593750000000000,0,1},{0.687500000000000,0,1},{0.781250000000000,0,1},{0.875000000000000,0,1},{0.968750000000000,0,1},{1,0,0.937500000000000},{1,0,0.843750000000000},{1,0,0.750000000000000},{1,0,0.656250000000000},{1,0,0.562500000000000},{1,0,0.468750000000000},{1,0,0.375000000000000},{1,0,0.281250000000000},{1,0,0.187500000000000},{1,0,0.0937500000000000}};


#endif
